/**
 * @brief 			Top.cpp Member Function Implementations
 */

#include "Top.h"
#include "FRules.h"
#include "NNet.h"
#include "Braitenberg.h"

#include "EPuck/epuck_dummy.hpp"
#include "EPuck/epuck_impl.hpp"
#include "EPuck/epuck_types.hpp"

#include <iostream>

Top::Top(const ControllerType& contr)
: epuck(chooseRobot())
, running(true)
{
  switch(contr)
  {
    case FACTS_RULES: controller = new FRules; break;
    case NEURONAL_NET: controller = new NNet; break;
    case BRAITENBERG: controller = new Braitenberg; break;
    default: throw;
  }
}

Top::~Top()
{
  running = false;
  delete controller;

  epuck->setMotorSpeed(0,0);
  usleep(1000);
}

gui::EpuckInterface* Top::chooseRobot()
{
  if(SIMULATION)
    return new gui::EpuckDummy();
  else
    return new gui::EpuckImpl(EPUCK_TYPE);
}

void Top::run()
{
  epuck->calibrateProximitySensors();

  while(running)
  {
    std::cout << "---------------------------------------------\n";
    readSensors();
    controller->input(sensors);

    double left = controller->output().getSpeedLeft();
    double right = controller->output().getSpeedRight();

    std::cout << "left: " << left << " right " << right << "\n";
    epuck->setMotorSpeed(left, right);

    usleep(WAIT_TIME);
  }
}

void Top::readSensors()
{
  gui::Int8 s;
  s = epuck->getProximitySensors();


  std::cout << "sensor:";
  for(int i = 0; i < NUM_SENS; ++i)
  {
    sensors[i] = s.data[i];
    std::cout << " i: " << i << " value: " << sensors[i] << "\n";
  }
}
